home *** CD-ROM | disk | FTP | other *** search
/ SGI Developer Toolbox 6.1 / SGI Developer Toolbox 6.1 - Disc 4.iso / src / demos / REALITY / fastshadows / matrix.c < prev    next >
C/C++ Source or Header  |  1994-08-01  |  6KB  |  230 lines
  1. /*
  2.  * Copyright 1993, 1994, Silicon Graphics, Inc.
  3.  * All Rights Reserved.
  4.  *
  5.  * This is UNPUBLISHED PROPRIETARY SOURCE CODE of Silicon Graphics, Inc.;
  6.  * the contents of this file may not be disclosed to third parties, copied or
  7.  * duplicated in any form, in whole or in part, without the prior written
  8.  * permission of Silicon Graphics, Inc.
  9.  *
  10.  * RESTRICTED RIGHTS LEGEND:
  11.  * Use, duplication or disclosure by the Government is subject to restrictions
  12.  * as set forth in subdivision (c)(1)(ii) of the Rights in Technical Data
  13.  * and Computer Software clause at DFARS 252.227-7013, and/or in similar or
  14.  * successor clauses in the FAR, DOD or NASA FAR Supplement. Unpublished -
  15.  * rights reserved under the Copyright Laws of the United States.
  16.  */
  17. /*
  18.  *    matrix -
  19.  *        Some utilities for working with matricies.
  20.  *
  21.  *                Paul Haeberli - 1985
  22.  */
  23. #include "stdio.h"
  24.  
  25. invertmat(from,to)
  26. float from[4][4],to[4][4];
  27. {
  28.     float wtemp[4][8];
  29.     float m0,m1,m2,m3,s;
  30.     float *r0,*r1,*r2,*r3, *rtemp;
  31.  
  32.     r0 = wtemp[0];
  33.     r1 = wtemp[1];
  34.     r2 = wtemp[2];
  35.     r3 = wtemp[3];
  36.     r0[0] = from[0][0];        /* build up [A][I]    */
  37.     r0[1] = from[0][1];
  38.     r0[2] = from[0][2];
  39.     r0[3] = from[0][3];
  40.     r0[4] = 1.0;
  41.     r0[5] = 0.0;
  42.     r0[6] = 0.0;
  43.     r0[7] = 0.0;
  44.     r1[0] = from[1][0];
  45.     r1[1] = from[1][1];
  46.     r1[2] = from[1][2];
  47.     r1[3] = from[1][3];
  48.     r1[4] = 0.0;
  49.     r1[5] = 1.0;
  50.     r1[6] = 0.0;
  51.     r1[7] = 0.0;
  52.     r2[0] = from[2][0];
  53.     r2[1] = from[2][1];
  54.     r2[2] = from[2][2];
  55.     r2[3] = from[2][3];
  56.     r2[4] = 0.0;
  57.     r2[5] = 0.0;
  58.     r2[6] = 1.0;
  59.     r2[7] = 0.0;
  60.     r3[0] = from[3][0];
  61.     r3[1] = from[3][1];
  62.     r3[2] = from[3][2];
  63.     r3[3] = from[3][3];
  64.     r3[4] = 0.0;
  65.     r3[5] = 0.0;
  66.     r3[6] = 0.0;
  67.     r3[7] = 1.0;
  68.  
  69.     if (r0[0] == 0.0) {        /* swap rows if needed        */
  70.     if (r1[0] == 0.0) {
  71.         if (r2[0] == 0.0) {
  72.         if (r3[0] == 0.0) goto singular;
  73.         rtemp = r0; r0 = r3; r3 = rtemp;
  74.         }
  75.         else {rtemp = r0; r0 = r2; r2 = rtemp;}
  76.     }
  77.     else {rtemp = r0; r0 = r1; r1 = rtemp;}
  78.     }
  79.     m1 = r1[0]/r0[0];        /* eliminate first variable    */
  80.     m2 = r2[0]/r0[0];
  81.     m3 = r3[0]/r0[0];
  82.     s = r0[1];
  83.     r1[1] = r1[1] - m1 * s;
  84.     r2[1] = r2[1] - m2 * s;
  85.     r3[1] = r3[1] - m3 * s;
  86.     s = r0[2];
  87.     r1[2] = r1[2] - m1 * s;
  88.     r2[2] = r2[2] - m2 * s;
  89.     r3[2] = r3[2] - m3 * s;
  90.     s = r0[3];
  91.     r1[3] = r1[3] - m1 * s;
  92.     r2[3] = r2[3] - m2 * s;
  93.     r3[3] = r3[3] - m3 * s;
  94.     s = r0[4];
  95.     if (s != 0.0) {
  96.     r1[4] = r1[4] - m1 * s;
  97.     r2[4] = r2[4] - m2 * s;
  98.     r3[4] = r3[4] - m3 * s;
  99.     }
  100.     s = r0[5];
  101.     if (s != 0.0) {
  102.     r1[5] = r1[5] - m1 * s;
  103.     r2[5] = r2[5] - m2 * s;
  104.     r3[5] = r3[5] - m3 * s;
  105.     }
  106.     s = r0[6];
  107.     if (s != 0.0) {
  108.     r1[6] = r1[6] - m1 * s;
  109.     r2[6] = r2[6] - m2 * s;
  110.     r3[6] = r3[6] - m3 * s;
  111.     }
  112.     s = r0[7];
  113.     if (s != 0.0) {
  114.     r1[7] = r1[7] - m1 * s;
  115.     r2[7] = r2[7] - m2 * s;
  116.     r3[7] = r3[7] - m3 * s;
  117.     }
  118.  
  119.     if (r1[1] == 0.0) {        /* swap rows if needed        */
  120.     if (r2[1] == 0.0) {
  121.         if (r3[1] == 0.0) goto singular;
  122.         rtemp = r1; r1 = r3; r3 = rtemp;
  123.     }
  124.     else {rtemp = r1; r1 = r2; r2 = rtemp;}
  125.     }
  126.     m2 = r2[1]/r1[1];        /* eliminate second variable    */
  127.     m3 = r3[1]/r1[1];
  128.     r2[2] = r2[2] - m2 * r1[2];
  129.     r3[2] = r3[2] - m3 * r1[2];
  130.     r3[3] = r3[3] - m3 * r1[3];
  131.     r2[3] = r2[3] - m2 * r1[3];
  132.     s = r1[4];
  133.     if (s != 0.0) {
  134.     r2[4] = r2[4] - m2 * s;
  135.     r3[4] = r3[4] - m3 * s;
  136.     }
  137.     s = r1[5];
  138.     if (s != 0.0) {
  139.     r2[5] = r2[5] - m2 * s;
  140.     r3[5] = r3[5] - m3 * s;
  141.     }
  142.     s = r1[6];
  143.     if (s != 0.0) {
  144.     r2[6] = r2[6] - m2 * s;
  145.     r3[6] = r3[6] - m3 * s;
  146.     }
  147.     s = r1[7];
  148.     if (s != 0.0) {
  149.     r2[7] = r2[7] - m2 * s;
  150.     r3[7] = r3[7] - m3 * s;
  151.     }
  152.  
  153.     if (r2[2] == 0.0) {        /* swap last 2 rows if needed    */
  154.     if (r3[2] == 0.0) goto singular;
  155.     rtemp = r2; r2 = r3; r3 = rtemp;
  156.     }
  157.     m3 = r3[2]/r2[2];        /* eliminate third variable    */
  158.     r3[3] = r3[3] - m3 * r2[3];
  159.     r3[4] = r3[4] - m3 * r2[4];
  160.     r3[5] = r3[5] - m3 * r2[5];
  161.     r3[6] = r3[6] - m3 * r2[6];
  162.     r3[7] = r3[7] - m3 * r2[7];
  163.  
  164.     if (r3[3] == 0.0) goto singular;
  165.     s = 1.0/r3[3];        /* now back substitute row 3    */
  166.     r3[4] = r3[4] * s;
  167.     r3[5] = r3[5] * s;
  168.     r3[6] = r3[6] * s;
  169.     r3[7] = r3[7] * s;
  170.  
  171.     m2 = r2[3];            /* now back substitute row 2    */
  172.     s = 1.0/r2[2];
  173.     r2[4] = s * (r2[4] - r3[4] * m2);
  174.     r2[5] = s * (r2[5] - r3[5] * m2);
  175.     r2[6] = s * (r2[6] - r3[6] * m2);
  176.     r2[7] = s * (r2[7] - r3[7] * m2);
  177.     m1 = r1[3];
  178.     r1[4] = (r1[4] - r3[4] * m1);
  179.     r1[5] = (r1[5] - r3[5] * m1);
  180.     r1[6] = (r1[6] - r3[6] * m1);
  181.     r1[7] = (r1[7] - r3[7] * m1);
  182.     m0 = r0[3];
  183.     r0[4] = (r0[4] - r3[4] * m0);
  184.     r0[5] = (r0[5] - r3[5] * m0);
  185.     r0[6] = (r0[6] - r3[6] * m0);
  186.     r0[7] = (r0[7] - r3[7] * m0);
  187.  
  188.     m1 = r1[2];            /* now back substitute row 1    */
  189.     s = 1.0/r1[1];
  190.     r1[4] = s * (r1[4] - r2[4] * m1);
  191.     r1[5] = s * (r1[5] - r2[5] * m1);
  192.     r1[6] = s * (r1[6] - r2[6] * m1);
  193.     r1[7] = s * (r1[7] - r2[7] * m1);
  194.     m0 = r0[2];
  195.     r0[4] = (r0[4] - r2[4] * m0);
  196.     r0[5] = (r0[5] - r2[5] * m0);
  197.     r0[6] = (r0[6] - r2[6] * m0);
  198.     r0[7] = (r0[7] - r2[7] * m0);
  199.  
  200.     m0 = r0[1];            /* now back substitute row 0    */
  201.     s = 1.0/r0[0];
  202.     r0[4] = s * (r0[4] - r1[4] * m0);
  203.     r0[5] = s * (r0[5] - r1[5] * m0);
  204.     r0[6] = s * (r0[6] - r1[6] * m0);
  205.     r0[7] = s * (r0[7] - r1[7] * m0);
  206.  
  207.     to[0][0] = r0[4];        /* copy results back        */
  208.     to[0][1] = r0[5];
  209.     to[0][2] = r0[6];
  210.     to[0][3] = r0[7];
  211.     to[1][0] = r1[4];
  212.     to[1][1] = r1[5];
  213.     to[1][2] = r1[6];
  214.     to[1][3] = r1[7];
  215.     to[2][0] = r2[4];
  216.     to[2][1] = r2[5];
  217.     to[2][2] = r2[6];
  218.     to[2][3] = r2[7];
  219.     to[3][0] = r3[4];
  220.     to[3][1] = r3[5];
  221.     to[3][2] = r3[6];
  222.     to[3][3] = r3[7];
  223.     return;
  224.  
  225. singular:
  226.     fprintf(stderr,"invertmat: singular matrix\n");
  227.     return;
  228. }
  229.  
  230.